/**
 * @file turn.c
 * @author sgfz
 * @brief 灰度转向
 * @version 0.1
 * @date 2024-09-12
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "turn.h"
#include "trace.h"
#include "bsp_adc.h"
#include "posture.h"
#include "bsp_SysTick.h"
#include "bsp_compass.h"
#include "reset.h"
#include "pid_turn.h"
#include "bsp_servo.h"

extern int cnt_hdl;
extern int cnt_hdr;

/*指南针平台掉头*/

/**
 * @brief 低平台掉头转180
 */
void Tai1_6_zhuan(void)
{
    Stop(100);
    HWT101_to_0();
    Stop(250);
    Front_up_High();
    // gqJD();
    // ZWCJD();
    // FWCJD();
    Deg_IN();
    for (int x = 50; x < 105; x++) {
        run(x * 0.93, -x * 1.0);
        delay_ms(8); //
    }
    for (int x = 50; x > 105; x--) {
        run(x * 0.93, -x * 1.0);
        delay_ms(8);
    }
    pid_Turn(1000);
    stop();
    Front_down();
}
/**
 * @brief 中平台掉头转180
 *
 */
void Tai7_zhuan(void)
{
    Stop(100);
    HWT101_to_0();
    Stop(250);
    Front_up_High();
    // gqJD();
    // ZWCJD();
    // FWCJD();
    Deg_IN();
    for (int x = 50; x < 105; x++) {
        run(x * 0.86, -x * 1.0);
        delay_ms(8); //
    }
    for (int x = 50; x > 105; x--) {
        run(x * 0.88, -x * 1.0);
        delay_ms(8);
    }
    pid_Turn(1000);
    Stop(50);
    Front_down();
}
/**
 * @brief 高平台掉头转180
 *
 */
void Tai8_zhuan(void)
{
    Front_up_High();
    Stop(100);
    HWT101_to_0();
    Stop(250);

    Deg_IN();
    for (int x = 40; x < 95; x++) {
        run(x * 0.96, -x * 1.05);
        delay_ms(9);
    }
    for (int x = 40; x > 95; x--) {
        run(x * 0.96, -x * 1.05);
        delay_ms(9);
    }
    pid_Turn(1100);
    stop();
    // Front_down();
}

/**
 * @brief 右转90度
 */
void TurnRight_90_Rdetect(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(0) > white || Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 96, 5);
    Right_Speed_Down(50, -85, 5);
    while (1) {
        run(70, -73);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}
/**
 * @brief 右转90度进景点
 *
 */
void TurnRightJD_90(void)
{
    // 检测右转
    while (1) {
        slow_run(50);
        if (Huidu_va(0) > white || Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    // 右转
    Left_Speed_Up(50, 96, 5);
    Right_Speed_Down(50, -90, 5);
    while (1) {
        run(80, -77);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}

/**
 * @brief 左转90度 右灰度检测
 *
 */
void TurnLeft_90_Rdetect(void)
{
    // 检测左转
    while (1) {
        slow_run(50);
        if (Huidu_va(11) > white || Huidu_va(10) > white || Huidu_va(9) > white) {
            break;
        }
    }
    // 左转
    Right_Speed_Up(50, 96, 5);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-75, 80);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

/**
 * @brief 左转90度 左灰度检测
 *
 */
void TurnLeft_90_Ldetect(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(0) > white || Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    Right_Speed_Up(50, 100, 5);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-70, 100);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}

/**
 * @brief 左转大角度 左灰度检测
 */
void TurnLeft_155(void)
{
    // 下8平台转
    while (Huidu_va(10) < white) {
        slow_run(50);
    }
    Right_Speed_Up(50, 120, 4);
    Left_Speed_Down(50, -90, 5);

    while (1) {
        run(-70, 100);
        if (Huidu_va(9) > white || Huidu_va(8) > white) {
            break;
        }
    } 
}

/**
 * @brief 原地差速左转
 */
void TurnLeft_Place_90(void)
{
    while (1) {
        run(-60, 75);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    while (1) {
        run(-65, 70);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}

/**
 * @brief 景点1 2掉头
 *
 */
void JD1_2_TurnAround(void)
{
    Right_Speed_Up(0, 80, 3);
    Left_Speed_Down(0, -80, 3);
    while (1) {
        run(-80, 80);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    while (1) {
        run(-60, 60);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}

/**
 * @brief 左转进景点1
 *
 */
void TurnLeft_inJD1(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    Right_Speed_Up(50, 120, 4);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-70, 105);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}

/**
 * @brief 右转上梯形山
 *
 */
void TurnRight_45_TXS(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(0) > white || Huidu_va(1) > white) {
            break;
        }
    }
    // 右转
    Right_Speed_Down(50, -90, 5);
    Left_Speed_Up(50, 90, 5);
}

/**
 * @brief 左转进长直线
 *
 */
void TurnLeft_45_Longline(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    // 左转
    Right_Speed_Up(50, 90, 15);
    while (1) {
        run(-50, 45);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}

/**
 * @brief 左转进景点2
 *
 */
void TurnLeft_inJD2(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    // 左转
    Left_Speed_Down(50, -90, 5);
    Right_Speed_Up(50, 100, 5);
}

/**
 * @brief 景点1顺拐进门2
 *
 */
void Small_Turn_inDoor2(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        high_run(90);
    }

    // while (1) {
    //     high_run(90);
    //     if (Huidu_va(1) > white || Huidu_va(0) > white) {
    //         break;
    //     }
    // }
    Right_Speed_Down(90, 45, 4);
    run_delay(90, 45, 280);
}
/**
 * @brief 转45度进台5直线
 *
 */
void TurnLeft_45_inTai5(void)
{
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        slow_run(50);
    }
    Right_Speed_Up(50, 100, 4);
    Left_Speed_Down(50, -90, 5);

    while (1) {
        run(-65, 80);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}
/**
 * @brief 原地差速右转
 */
void TurnRight_Place_90(void)
{
    while (1) {
        run(60, -60);
        if (Huidu_va(1) > white || Huidu_va(0) > white) {
            break;
        }
    }
    while (1) {
        run(50, -55);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}
/**
 * @brief 右转45度进交叉口
 */
void TurnRight_45_inCross(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(0) > white || Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 80, 5);
    Right_Speed_Down(50, -80, 5);
    while (1) {
        run(60, -63);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void TurnLeft_155_atCross(void)
{
    while (Huidu_va(10) < white) {
        slow_run(50);
    }
    Right_Speed_Up(50, 120, 4);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-70, 100);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}
/**
 * @brief 门2右转大角度出门区进入长直线
 *
 */
void TurnRight_155_outCross(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(1) > white || Huidu_va(0) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 120, 4);
    Right_Speed_Down(50, -90, 5);
    while (1) {
        run(90, -70);
        if (Huidu_va(4) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

/**
 * @brief 右转大角度进门3
 *
 */
void TurnRight_155_door3(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(1) > white || Huidu_va(0) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 120, 4);
    Right_Speed_Down(50, -90, 5);
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        run(90, -70);
    }
    while (1) {
        run(90, -70);
        if (Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    while (1) {
        run(90, -70);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}

/**
 * @brief 门12转入波浪板
 *
 */
void door12_Turn_BLB(void)
{
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        slow_run(50);
    }

    Left_Speed_Down(50, -90, 5);
    Right_Speed_Up(50, 90, 5);
    while (1) {
        run(-55, 50);
        if (Huidu_va(5) > white || Huidu_va(6) > white) {
            break;
        }
    }
}

void TurnRight_45_Tai1(void)
{
    while (1) {
        slow_run(60);
        if (Huidu_va(1) > white || Huidu_va(0) > white) {
            break;
        }
    }

    Right_Speed_Down(50, -90, 5);
    Left_Speed_Up(50, 90, 4);
}

void TurnRight_90_Ldetect(void)
{

    while (1) {
        slow_run(50);
        if (Huidu_va(11) > white || Huidu_va(10) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 96, 5);
    Right_Speed_Down(50, -85, 5);
    while (1) {
        run(70, -73);
        if (Huidu_va(4) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void TurnRight_90_hdrDetect(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        slow_run(50);
    }

    Right_Speed_Down(50, -90, 5);
    Left_Speed_Up(50, 85, 4);

    while (1) {
        run(80, -60);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void TurnLeft_155_hdlDetect(void)
{
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        slow_run(50);
    }
    Left_Speed_Down(50, -90, 5);
    Right_Speed_Up(50, 85, 4);
    while (1) {
        run(-60, 80);
        if (Huidu_va(0) > white || Huidu_va(1) > white) {
            break;
        }
    }
    while (1) {
        run(-70, 60);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void TurnLeft_Door1_inCross(void)
{
    while (Huidu_va(10) < white) {
        slow_run(50);
    }
    Right_Speed_Up(50, 120, 5);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-70, 95);
        if (Huidu_va(0) > white || Huidu_va(1) > white) {
            break;
        }
    }
    while (1) {
        run(-70, 85);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void door34_Turn_BLB(void)
{
    while (1) {
        slow_run(50);
        if (Huidu_va(1) > white || Huidu_va(0) > white) {
            break;
        }
    }
    Left_Speed_Up(50, 120, 4);
    Right_Speed_Down(50, -90, 5);
    while (1) {
        run(90, -70);
        if (Huidu_va(1) > white || Huidu_va(2) > white) {
            break;
        }
    }
    while (1) {
        run(90, -70);
        if (Huidu_va(6) > white || Huidu_va(5) > white) {
            break;
        }
    }
}

void TurnLeft_155_door4(void)
{
    while (Huidu_va(10) < white) {
        slow_run(50);
    }
    Right_Speed_Up(50, 120, 4);
    Left_Speed_Down(50, -90, 5);
    while (1) {
        run(-70, 100);
        if (Huidu_va(6) > white || Huidu_va(7) > white) {
            break;
        }
    }
}

void Small_Turn_inline(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        high_run(90);
    }

    // while (1) {
    //     high_run(90);
    //     if (Huidu_va(1) > white || Huidu_va(0) > white) {
    //         break;
    //     }
    // }
    Left_Speed_Down(90, 45, 4);
    run_delay(45, 90, 210);
}

void TurnLeft_inDoor1(void)
{
    cnt_hdr = 0;
    while (cnt_hdr == 0) {
        high_run(90);
    }
    Left_Speed_Up(90, 110, 4);
    Right_Speed_Down(90, -90, 4);
    cnt_hdl = 0;
    while (cnt_hdl == 0) {
        run(85, -77);
    }
    while (1) {
        run(80, -70);
        if (Huidu_va(7) > white || Huidu_va(8) > white) {
            break;
        }
    }

    Reset(60, 60);
    speed_up(60, 140);
    speed_down(140, 60);
}